
#include "sys_pub.h"
#include "modbus_rtu.h"
#include <string.h>

// -------------------------- Private Utility Functions --------------------------
/**
 * @brief Calculate Modbus CRC16 checksum
 * @param data Pointer to data for checksum calculation
 * @param length Length of data in bytes
 * @return Calculated CRC16 value
 */
static uint16_t modbus_crc16(const uint8_t* data, uint16_t length) {
    uint16_t crc = 0xFFFF;
    for (uint16_t i = 0; i < length; i++) {
        crc ^= (uint16_t)data[i];
        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc = (crc >> 1) ^ 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    return crc;
}

/**
 * @brief Pack Modbus frame (convert structure to byte stream)
 * @param frame Pointer to ModbusFrame structure
 * @param buffer Output buffer for packed data
 * @return Length of packed data in bytes
 */
static uint16_t modbus_pack_frame(const ModbusFrame* frame, uint8_t* buffer) {
    if (!frame || !buffer) return 0;
    
    uint16_t pos = 0;
    buffer[pos++] = frame->address;
    buffer[pos++] = (uint8_t)frame->function;
    
    memcpy(&buffer[pos], frame->data, frame->data_length);
    pos += frame->data_length;
    
    // Calculate and append CRC (little-endian format)
    uint16_t crc = modbus_crc16(buffer, pos);
    buffer[pos++] = (uint8_t)(crc & 0xFF);
    buffer[pos++] = (uint8_t)(crc >> 8);
    
    return pos;
}

/**
 * @brief Unpack Modbus frame (parse byte stream into structure)
 * @param buffer Input buffer containing raw frame data
 * @param length Length of data in buffer
 * @param frame Output ModbusFrame structure
 * @return Unpacking result (true=success)
 */
static bool modbus_unpack_frame(const uint8_t* buffer, uint16_t length, ModbusFrame* frame) {
    if (!buffer || !frame || length < MODBUS_MIN_FRAME_LENGTH) return false;
    
    // Verify CRC checksum
    uint16_t received_crc = (buffer[length - 1] << 8) | buffer[length - 2];
    uint16_t calculated_crc = modbus_crc16(buffer, length - MODBUS_CRC_LENGTH);
    if (received_crc != calculated_crc) return false;
    
    // Parse frame structure
    frame->address = buffer[0];
    frame->function = (ModbusFunctionCode)buffer[1];
    frame->data_length = length - 3;  // Subtract address(1) + function(1) + CRC(2)
    memcpy(frame->data, &buffer[2], frame->data_length);
    frame->crc = received_crc;
    
    return true;
}

/**
 * @brief Send exception response
 * @param ctx Modbus context pointer
 * @param callbacks Pointer to callback interface structure
 * @param address Target device address
 * @param function Function code of original request
 * @param exception Exception code to send
 */
static void modbus_send_exception(ModbusContext* ctx, const ModbusCallbacks* callbacks,
                                 uint8_t address, ModbusFunctionCode function,
                                 ModbusExceptionCode exception) {
    if (!ctx || !callbacks || !callbacks->send) return;
    
    ModbusFrame frame = {
        .address = address,
        .function = (ModbusFunctionCode)((uint8_t)function | 0x80),  // Set exception bit
        .data_length = 1,
        .data = {(uint8_t)exception}
    };
    
    uint8_t buffer[MODBUS_MAX_FRAME_LENGTH];
    uint16_t length = modbus_pack_frame(&frame, buffer);
    callbacks->send(buffer, length);
}

// -------------------------- Frame Processing Functions --------------------------
/**
 * @brief Handle read coils request (0x01)
 */
static void modbus_handle_read_coils(ModbusContext* ctx, const ModbusCallbacks* callbacks,
                                    const ModbusFrame* request, ModbusFrame* response) {
    if (!ctx || !callbacks || !request || !response || !callbacks->coil_handler) {
        return;
    }
    
    // Parse request parameters (start address + quantity)
    uint16_t start_addr = (request->data[0] << 8) | request->data[1];
    uint16_t coil_count = (request->data[2] << 8) | request->data[3];
    
    // Validate parameters
    if (coil_count == 0 || coil_count > 0x07D0) {  // Max 2000 coils per Modbus spec
        modbus_send_exception(ctx, callbacks, request->address, request->function,
                             MODBUS_EXCEPTION_ILLEGAL_DATA);
        return;
    }
    
    // Prepare response frame
    response->address = request->address;
    response->function = request->function;
    response->data_length = (coil_count + 7) / 8 + 1;  // Bytes + count byte
    response->data[0] = (uint8_t)response->data_length - 1;  // Byte count
    
    // Read coil statuses
    bool coil_value;
    for (uint16_t i = 0; i < coil_count; i++) {
        if (!callbacks->coil_handler(start_addr + i, &coil_value, false)) {
            modbus_send_exception(ctx, callbacks, request->address, request->function,
                                 MODBUS_EXCEPTION_ILLEGAL_ADDR);
            return;
        }
        if (coil_value) {
            response->data[1 + (i / 8)] |= (1 << (i % 8));
        }
    }
}

/**
 * @brief Handle read holding registers request (0x03)
 */
static void modbus_handle_read_holding_registers(ModbusContext* ctx, const ModbusCallbacks* callbacks,
                                                const ModbusFrame* request, ModbusFrame* response) {
    if (!ctx || !callbacks || !request || !response || !callbacks->register_handler) {
        return;
    }
    
    // Parse request parameters
    uint16_t start_addr = (request->data[0] << 8) | request->data[1];
    uint16_t reg_count = (request->data[2] << 8) | request->data[3];
    
    // Validate parameters
    if (reg_count == 0 || reg_count > 0x007D) {  // Max 125 registers per Modbus spec
        modbus_send_exception(ctx, callbacks, request->address, request->function,
                             MODBUS_EXCEPTION_ILLEGAL_DATA);
        return;
    }
    
    // Prepare response frame
    response->address = request->address;
    response->function = request->function;
    response->data_length = reg_count * 2 + 1;  // 2 bytes/register + count byte
    response->data[0] = (uint8_t)(reg_count * 2);  // Byte count
    
    // Read register values
    uint16_t reg_value;
    for (uint16_t i = 0; i < reg_count; i++) {
        if (!callbacks->register_handler(start_addr + i, &reg_value, false)) {
            modbus_send_exception(ctx, callbacks, request->address, request->function,
                                 MODBUS_EXCEPTION_ILLEGAL_ADDR);
            return;
        }
        response->data[1 + i * 2] = (uint8_t)(reg_value >> 8);  // High byte first
        response->data[2 + i * 2] = (uint8_t)(reg_value & 0xFF);
    }
}

/**
 * @brief Handle write single coil request (0x05)
 */
static void modbus_handle_write_coil(ModbusContext* ctx, const ModbusCallbacks* callbacks,
                                    const ModbusFrame* request, ModbusFrame* response) {
    if (!ctx || !callbacks || !request || !response || !callbacks->coil_handler) {
        return;
    }
    
    // Parse request parameters
    uint16_t coil_addr = (request->data[0] << 8) | request->data[1];
    bool coil_value = (request->data[2] == 0xFF) ? true : false;  // 0xFF00=1, 0x0000=0 per spec
    
    // Perform write operation
    if (!callbacks->coil_handler(coil_addr, &coil_value, true)) {
        modbus_send_exception(ctx, callbacks, request->address, request->function,
                             MODBUS_EXCEPTION_ILLEGAL_ADDR);
        return;
    }
    
    // Prepare response frame (echo request parameters)
    response->address = request->address;
    response->function = request->function;
    response->data_length = 4;
    memcpy(response->data, request->data, 4);
}

/**
 * @brief Handle write single register request (0x06)
 */
static void modbus_handle_write_register(ModbusContext* ctx, const ModbusCallbacks* callbacks,
                                        const ModbusFrame* request, ModbusFrame* response) {
    if (!ctx || !callbacks || !request || !response || !callbacks->register_handler) {
        return;
    }
    
    // Parse request parameters
    uint16_t reg_addr = (request->data[0] << 8) | request->data[1];
    uint16_t reg_value = (request->data[2] << 8) | request->data[3];
    
    // Perform write operation
    if (!callbacks->register_handler(reg_addr, &reg_value, true)) {
        modbus_send_exception(ctx, callbacks, request->address, request->function,
                             MODBUS_EXCEPTION_ILLEGAL_ADDR);
        return;
    }
    
    // Prepare response frame (echo request parameters)
    response->address = request->address;
    response->function = request->function;
    response->data_length = 4;
    memcpy(response->data, request->data, 4);
}

// -------------------------- Public Interface Functions Implementation --------------------------
void modbus_init(ModbusContext* ctx, uint8_t slave_addr, uint32_t tick_interval, uint32_t timeout) {
    if (!ctx) return;
    
    memset(ctx, 0, sizeof(ModbusContext));
    ctx->slave_address = slave_addr;
    ctx->tick_interval_ms = tick_interval;
    ctx->frame_timeout_ms = timeout;
}

void modbus_receive_byte(ModbusContext* ctx, uint8_t data) {
    if (!ctx) return;
    
    // Check for buffer overflow
    if (ctx->rx_state.length >= MODBUS_MAX_FRAME_LENGTH) {
        ctx->rx_state.overflow = true;
        return;
    }
    
    // Store data and reset timeout counter
    ctx->rx_state.buffer[ctx->rx_state.length++] = data;
    ctx->rx_state.timeout_counter = 0;
    ctx->rx_state.frame_complete = false;
}

void modbus_tick(ModbusContext* ctx) {
    if (!ctx || ctx->rx_state.length == 0) return;
    
    ctx->rx_state.timeout_counter += ctx->tick_interval_ms;
    if (ctx->rx_state.timeout_counter >= ctx->frame_timeout_ms) {
        ctx->rx_state.frame_complete = true;  // Mark frame as complete on timeout
    }
}

void modbus_process_frame(ModbusContext* ctx, const ModbusCallbacks* callbacks) {
    if (!ctx || !callbacks || !ctx->rx_state.frame_complete) return;
    
    ModbusFrame request, response;
    uint8_t send_buffer[MODBUS_MAX_FRAME_LENGTH];
    uint16_t send_length = 0;
    
    // Handle buffer overflow
    if (ctx->rx_state.overflow) {
        goto reset_state;
    }
    
    // Unpack and validate frame
    if (!modbus_unpack_frame(ctx->rx_state.buffer, ctx->rx_state.length, &request)) {
        goto reset_state;
    }
    
    // Check address (local address or broadcast)
    if (request.address != ctx->slave_address && request.address != MODBUS_BROADCAST_ADDR) {
        goto reset_state;
    }
    
    // Process function code
    memset(&response, 0, sizeof(ModbusFrame));
    switch (request.function) {
        case MODBUS_FUNC_READ_COILS:
            modbus_handle_read_coils(ctx, callbacks, &request, &response);
            send_length = modbus_pack_frame(&response, send_buffer);
            break;
            
        case MODBUS_FUNC_READ_HOLDING:
            modbus_handle_read_holding_registers(ctx, callbacks, &request, &response);
            send_length = modbus_pack_frame(&response, send_buffer);
            break;
            
        case MODBUS_FUNC_WRITE_COIL:
            modbus_handle_write_coil(ctx, callbacks, &request, &response);
            send_length = modbus_pack_frame(&response, send_buffer);
            break;
            
        case MODBUS_FUNC_WRITE_REGISTER:
            modbus_handle_write_register(ctx, callbacks, &request, &response);
            send_length = modbus_pack_frame(&response, send_buffer);
            break;
            
        default:
            // Illegal function code
            modbus_send_exception(ctx, callbacks, request.address, request.function,
                                 MODBUS_EXCEPTION_ILLEGAL_FUNC);
            break;
    }
    
    // Send response (no response for broadcast messages)
    if (request.address != MODBUS_BROADCAST_ADDR && send_length > 0 && callbacks->send) {
        callbacks->send(send_buffer, send_length);
    }
    
reset_state:
    // Reset receive state for next frame
    memset(&ctx->rx_state, 0, sizeof(ModbusRxState));
}

void modbus_send_request(ModbusContext* ctx, const ModbusCallbacks* callbacks,
                        uint8_t addr, ModbusFunctionCode func, const uint8_t* data, uint16_t data_len) {
    if (!ctx || !callbacks || !callbacks->send || !data || data_len == 0) return;
    
    ModbusFrame frame = {
        .address = addr,
        .function = func,
        .data_length = (data_len > MODBUS_MAX_FRAME_LENGTH - 3) ? (MODBUS_MAX_FRAME_LENGTH - 3) : data_len
    };
    memcpy(frame.data, data, frame.data_length);
    
    uint8_t buffer[MODBUS_MAX_FRAME_LENGTH];
    uint16_t length = modbus_pack_frame(&frame, buffer);
    callbacks->send(buffer, length);
}
    